/*    rpg_quadrotor_mpc
 *    A model predictive control implementation for quadrotors.
 *    Copyright (C) 2017-2018 Philipp Foehn, 
 *    Robotics and Perception Group, University of Zurich
 * 
 *    Intended to be used with rpg_quadrotor_control and rpg_quadrotor_common.
 *    https://github.com/uzh-rpg/rpg_quadrotor_control
 *
 *    This program is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 3 of the License, or
 *    (at your option) any later version.
 *
 *    This program is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */


#include <memory>
#include <acado_optimal_control.hpp>
#include <acado_code_generation.hpp>
#include <acado_gnuplot.hpp>
//#include <Eigen/Dense>
#include <cmath>

const int pc_size=30;

int main( ){
  // Use Acado
  USING_NAMESPACE_ACADO
  using namespace Eigen;


  const bool CODE_GEN = true;

  // System variables
  DifferentialState     p_1, p_5, p_6;//position
  DifferentialState     v_1, v_5, v_6;//velocity
  Control               a_1, a_5, a_6;//acceleration
  DifferentialEquation  f;               
  Function              h, hN;

  OnlineData            bx,by,bz; //position of drone
  OnlineData            fx[pc_size],fy[pc_size],fz[pc_size];//position of features


  // Parameters with exemplary values. These are set/overwritten at runtime.
  const double t_start = 0.0;     // Initial time [s]
  const double t_end = 0.2;       // Time horizon [s]
  const double dt = 0.01;          // Discretization time [s]
  const int N = round(t_end/dt);  // Number of nodes
  
  const double v_max=1.0;
  const double a_max=2.0;
  const int Ni = 1;

  //for visibility function
  const double k2=-0.24;
  const double k1= 0.44;
  const double k0= 0.69;

  const double eps=0.0001;

  // System Dynamics
  f << dot(p_1) ==  v_1;
  f << dot(p_5) ==  v_5;
  f << dot(p_6) ==  v_6;
  //f << dot(p_yaw) == v_yaw;

  f << dot(v_1) ==  a_1;
  f << dot(v_5) ==  a_5;
  f << dot(v_6) ==  a_6;


  //intial view vector:[1,0,0],take a zyz rotation

  IntermediateState view_x=cos(p_1)*cos(p_5)*cos(p_6)-sin(p_1)*sin(p_6);
  IntermediateState view_y=sin(p_1)*cos(p_5)*cos(p_6)+cos(p_1)*sin(p_6);
  IntermediateState view_z=-sin(p_5)*sin(p_6);

  IntermediateState fx_B,fy_B,fz_B,f_dist,cos_bear,visibility;
  IntermediateState cost=0;

  for(int i=0;i<pc_size;++i){

     fx_B=fx[i]-bx;
     fy_B=fy[i]-by;
     fz_B=fz[i]-bz;
     f_dist=sqrt(pow(fx_B,2)+pow(fy_B,2)+pow(fz_B,2));
     cos_bear=(view_x*fx_B+view_y*fy_B+view_z*fz_B)/f_dist;
     visibility=k2*cos_bear*cos_bear+k1*cos_bear+k0;
     
     cost+=-log(visibility)*10000;

    
  }


  //IntermediateState visibility=k2*cos_bear*cos_bear+k1*cos_bear+k0;



    h<<cost << v_1 << v_5 << v_6 << a_1 << a_5 << a_6 ;

  // End cost vector consists of all states (no inputs at last state).
    hN  <<cost<< v_1 << v_5 << v_6;




  // DEFINE AN OPTIMAL CONTROL PROBLEM:
  // ----------------------------------
  OCP ocp( t_start, t_end, N );

    // For code generation, references are set during run time.
    BMatrix Q_sparse(h.getDim(), h.getDim());
    Q_sparse.setIdentity();
    BMatrix QN_sparse(hN.getDim(), hN.getDim());
    QN_sparse.setIdentity();
    ocp.minimizeLSQ( Q_sparse, h);
    ocp.minimizeLSQEndTerm( QN_sparse, hN );
  

  // Add system dynamics
  ocp.subjectTo( f );
  // Add constraints
  ocp.subjectTo(a_max <= a_1 <= a_max);
  ocp.subjectTo(-a_max <= a_5 <= a_max);
  ocp.subjectTo(-a_max <= a_6 <= a_max);

  ocp.subjectTo(-v_max <= v_1 <= v_max);
  ocp.subjectTo(-v_max <= v_5 <= v_max);
  ocp.subjectTo(-v_max <= v_6 <= v_max);

  ocp.setNOD(93);

  

  
    // For code generation, we can set some properties.
    // The main reason for a setting is given as comment.
    OCPexport mpc(ocp);

        mpc.set(HESSIAN_APPROXIMATION, GAUSS_NEWTON);
        //mpc.set(DISCRETIZATION_TYPE, SINGLE_SHOOTING);        
        mpc.set(DISCRETIZATION_TYPE, MULTIPLE_SHOOTING);
        //mpc.set(INTEGRATOR_TYPE, INT_RK4);
        mpc.set(INTEGRATOR_TYPE, INT_IRK_RIIA3);
        mpc.set(NUM_INTEGRATOR_STEPS, N * 100);
        mpc.set(SPARSE_QP_SOLUTION, FULL_CONDENSING);
        //	mpc.set(SPARSE_QP_SOLUTION, CONDENSING);
        mpc.set(QP_SOLVER, QP_QPOASES);
        //	mpc.set(QP_SOLVER, QP_FORCES);
        //	mpc.set(MAX_NUM_QP_ITERATIONS, 20);
        mpc.set(HOTSTART_QP, YES);        
        //	mpc.set(SPARSE_QP_SOLUTION, SPARSE_SOLVER);        
        //	mpc.set(LEVENBERG_MARQUARDT, 1.0e-10);
        mpc.set(GENERATE_TEST_FILE, YES);
        mpc.set(GENERATE_MAKE_FILE, YES);
        mpc.set(GENERATE_MATLAB_INTERFACE, YES);
        //	mpc.set(USE_SINGLE_PRECISION, YES);
        mpc.set(CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
        //mpc.set( CG_HARDCODE_CONSTRAINT_VALUES, NO);
        //	mpc.set(CG_USE_OPENMP, YES);
        // NOTE: This is crucial for export of MHE!
	      //mpc.set(SPARSE_QP_SOLUTION, CONDENSING);
	      mpc.set(FIX_INITIAL_STATE, YES);

    // Finally, export everything.
    if(mpc.exportCode("quadrotor_mpc_codegen") != SUCCESSFUL_RETURN)
      exit( EXIT_FAILURE );
    mpc.printDimensionsQP( );
  
    
  return EXIT_SUCCESS;
}
